#include "Map.h"

#include <mutex>

namespace msf
{

long unsigned int Map::nNextId = 0;

Map::Map()
    : mnMaxKPFid(0), mnBigChangeIdx(0), mbImuInitialized(false), mnMapChange(0), mpFirstRegionKPF(static_cast<KeyPoseFrame *>(NULL)),
    mbFail(false), mIsInUse(false), mHasTumbnail(false), mbBad(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false)
{
    mnId = nNextId++;
    mThumbnail = static_cast<GLubyte *>(NULL);
}

Map::Map(int initKPFid)
    : mnInitKPFid(initKPFid), mnMaxKPFid(initKPFid), mnBigChangeIdx(0), mIsInUse(false),
    mHasTumbnail(false), mbBad(false), mbImuInitialized(false), mpFirstRegionKPF(static_cast<KeyPoseFrame *>(NULL)),
    mnMapChange(0), mbFail(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false)
{
    mnId = nNextId++;
    mThumbnail = static_cast<GLubyte *>(NULL);
}

Map::~Map()
{
    // TODO: erase all keyframes from memory
    mspKeyPoseFrames.clear();

    if (mThumbnail)
        delete mThumbnail;
    mThumbnail = static_cast<GLubyte *>(NULL);

    mvpKeyPoseFrameOrigins.clear();
}

// 在地图中插入关键帧,同时更新关键帧的最大id
void Map::AddKeyPoseFrame(KeyPoseFrame *pKPF)
{
    //Problem2 tracking和localoptim的构造函数不完整
    unique_lock<mutex> lock(mMutexMap);
    if (mspKeyPoseFrames.empty())
    {
        cout << "First KPF:" << pKPF->mnId << "; Map init KPF:" << mnInitKPFid << endl;
        mnInitKPFid = pKPF->mnId;
        mpKPFinitial = pKPF;
        mpKPFlowerID = pKPF;
    }
    mspKeyPoseFrames.insert(pKPF);
    if (pKPF->mnId > mnMaxKPFid)
    {
        mnMaxKPFid = pKPF->mnId;
    }
    if (pKPF->mnId < mpKPFlowerID->mnId)
    {
        mpKPFlowerID = pKPF;
    }
}

void Map::SetImuInitialized()
{
    unique_lock<mutex> lock(mMutexMap);
    mbImuInitialized = true;
}

bool Map::isImuInitialized()
{
    unique_lock<mutex> lock(mMutexMap);
    return mbImuInitialized;
}

void Map::EraseKeyPoseFrame(KeyPoseFrame *pKPF)
{
    unique_lock<mutex> lock(mMutexMap);
    mspKeyPoseFrames.erase(pKPF);
    if (mspKeyPoseFrames.size() > 0)
    {
        if (pKPF->mnId == mpKPFlowerID->mnId)
        {
            vector<KeyPoseFrame *> vpKPFs = vector<KeyPoseFrame *>(mspKeyPoseFrames.begin(), mspKeyPoseFrames.end());
            sort(vpKPFs.begin(), vpKPFs.end(), KeyPoseFrame::lId);
            mpKPFlowerID = vpKPFs[0];
        }
    }
    else
    {
        mpKPFlowerID = 0;
    }
}

//??
void Map::InformNewBigChange()
{
    unique_lock<mutex> lock(mMutexMap);
    mnBigChangeIdx++;
}

//??
int Map::GetLastBigChangeIdx()
{
    unique_lock<mutex> lock(mMutexMap);
    return mnBigChangeIdx;
}

// 获取地图中的所有关键帧
vector<KeyPoseFrame *> Map::GetAllKeyPoseFrames()
{
    unique_lock<mutex> lock(mMutexMap);
    return vector<KeyPoseFrame *>(mspKeyPoseFrames.begin(), mspKeyPoseFrames.end());
}

// 获取地图中的关键帧数目
long unsigned int Map::KeyPoseFramesInMap()
{
    unique_lock<mutex> lock(mMutexMap);
    return mspKeyPoseFrames.size();
}

long unsigned int Map::GetId()
{
    return mnId;
}

long unsigned int Map::GetInitKPFid()
{
    unique_lock<mutex> lock(mMutexMap);
    return mnInitKPFid;
}

void Map::SetInitKPFid(long unsigned int initKPFif)
{
    unique_lock<mutex> lock(mMutexMap);
    mnInitKPFid = initKPFif;
}

long unsigned int Map::GetMaxKPFid()
{
    unique_lock<mutex> lock(mMutexMap);
    return mnMaxKPFid;
}

KeyPoseFrame *Map::GetOriginKPF()
{
    return mpKPFinitial;
}

void Map::SetCurrentMap()
{
    mIsInUse = true;
}

void Map::SetStoredMap()
{
    mIsInUse = false;
}

void Map::clear()
{
    //    for(set<MapPoint*>::iterator sit=mspMapPoints.begin(), send=mspMapPoints.end(); sit!=send; sit++)
    //        delete *sit;

    for (set<KeyPoseFrame *>::iterator sit = mspKeyPoseFrames.begin(), send = mspKeyPoseFrames.end(); sit != send; sit++)
    {
        KeyPoseFrame *pKPF = *sit;
        pKPF->UpdateMap(static_cast<Map *>(NULL));
        //        delete *sit;
    }

    mspKeyPoseFrames.clear();
    mnMaxKPFid = mnInitKPFid;
    mbImuInitialized = false;
    mvpKeyPoseFrameOrigins.clear();
    mbIMU_BA1 = false;
    mbIMU_BA2 = false;
}

bool Map::IsInUse()
{
    return mIsInUse;
}

void Map::SetBad()
{
    mbBad = true;
}

bool Map::IsBad()
{
    return mbBad;
}

// 恢复尺度及重力方向
/** imu在localoptimizing中初始化，LocalOptimizing::InitializeIMU中使用，误差包含三个残差与两个偏置
 * 地图融合时也会使用
 * @param R 初始化时为Rgw
 * @param s 尺度
 * @param bScaledVel 将尺度更新到速度
 * @param t 默认cv::Mat::zeros(cv::Size(1,3),CV_32F)
 */
void Map::ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel)
{
    unique_lock<mutex> lock(mMutexMap);

    // Body position (IMU) of first keyframe is fixed to (0,0,0)
    Sophus::SE3f Tyw = T;
    Eigen::Matrix3f Ryw = Tyw.rotationMatrix();
    Eigen::Vector3f tyw = Tyw.translation();

    for (set<KeyPoseFrame *>::iterator sit = mspKeyPoseFrames.begin(); sit != mspKeyPoseFrames.end(); sit++)
    {
        // 更新关键帧位姿
        /**
         * | Rw2w1  tw2w1 |   *   | Rw1c  s*tw1c  |     =    |  Rw2c     s*Rw2w1*tw1c + tw2w1  |
         * |   0      1   |       |  0       1    |          |   0                1            |
         * 这么做比正常乘在旋转上少了个s，后面不需要这个s了，因为所有mp在下面已经全部转到了w2坐标系下，不存在尺度变化了
         * 
         * | s*Rw2w1  tw2w1 |   *   | Rw1c    tw1c  |     =    |  s*Rw2c     s*Rw2w1*tw1c + tw2w1  |
         * |   0        1   |       |  0       1    |          |     0                1            |
         */
        KeyPoseFrame *pKPF = *sit;
        Sophus::SE3f Two = pKPF->GetPoseInverse();
        Two.translation() *= s;

        // |  Ryc     s*Ryw*twc + tyw  |
        // |   0           1           |
        Sophus::SE3f Tyo = Tyw * Two;
        Sophus::SE3f Toy = Tyo.inverse();
        pKPF->SetPose(Toy);
        // 更新关键帧速度
        Eigen::Vector3f Vw = pKPF->GetVelocity();
        if (!bScaledVel)
            pKPF->SetVelocity(Ryw * Vw);
        else
            pKPF->SetVelocity(Ryw * Vw * s);
    }
    mnMapChange++;
}

void Map::SetInertialSensor()
{
    unique_lock<mutex> lock(mMutexMap);
    mbIsInertial = true;
}

bool Map::IsInertial()
{
    unique_lock<mutex> lock(mMutexMap);
    // 将mbIsInertial设置为true,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关
    return mbIsInertial;
}

void Map::SetIniertialBA1()
{
    unique_lock<mutex> lock(mMutexMap);
    mbIMU_BA1 = true;
}

void Map::SetIniertialBA2()
{
    unique_lock<mutex> lock(mMutexMap);
    mbIMU_BA2 = true;
}

bool Map::GetIniertialBA1()
{
    unique_lock<mutex> lock(mMutexMap);
    return mbIMU_BA1;
}

bool Map::GetIniertialBA2()
{
    unique_lock<mutex> lock(mMutexMap);
    return mbIMU_BA2;
}

void Map::ChangeId(long unsigned int nId)
{
    mnId = nId;
}

unsigned int Map::GetLowerKPFID()
{
    unique_lock<mutex> lock(mMutexMap);
    if (mpKPFlowerID)
    {
        return mpKPFlowerID->mnId;
    }
    return 0;
}

int Map::GetMapChangeIndex()
{
    unique_lock<mutex> lock(mMutexMap);
    return mnMapChange;
}

void Map::IncreaseChangeIndex()
{
    unique_lock<mutex> lock(mMutexMap);
    mnMapChange++;
}

int Map::GetLastMapChange()
{
    unique_lock<mutex> lock(mMutexMap);
    return mnMapChangeNotified;
}

void Map::SetLastMapChange(int currentChangeId)
{
    unique_lock<mutex> lock(mMutexMap);
    mnMapChangeNotified = currentChangeId;
}

} // namespace msf